Look Away

To see a Subscriber in action, you'll write a node called look_away. The look_away node will subscribe to the /rgb_camera/image_raw topic, which has image data from the camera mounted on the end of the robotic arm. Whenever the camera is pointed towards an uninteresting image - in this case, an image with uniform color - the callback function will move the arm to something more interesting. There are a few extra pieces in the code to ensure that this procedure is executed smoothly, but you will learn more about them later.

Creating the empty look_away node script

Just as before when you created the arm_mover and simple_mover nodes, you can create the look_away node as follows:

$ cd ~/catkin_ws
$ cd src/simple_arm/scripts
$ touch look_away
$ chmod u+x look_away

Let's have a look at the code for look_away.

Troubleshooting look_away

In some cases look_away is executing when running it manually but is not executing automatically with roslaunch. This is typically a timing issue. If look_away starts before the system has fully initialized, then look_away hangs in the call to safe_move. Student jsteinbae offered a great solution to this issue:

My workaround was to add wait_for_message to the look_away node before subscribing to the topics. This ensures that the callbacks are not called before the gazebo simulation (publishing these topics) is fully initialized.

def __init__(self):
    rospy.init_node('look_away')
    self.last_position = None
    self.arm_moving = False

    rospy.wait_for_message('/simple_arm/joint_states', JointState)
    rospy.wait_for_message('/rgb_camera/image_raw', Image)

    self.sub1 = rospy.Subscriber('/simple_arm/joint_states', 
                                 JointState, self.joint_states_callback)
    self.sub2 = rospy.Subscriber('/rgb_camera/image_raw', 
                                 Image, self.look_away_callback)
    self.safe_move = rospy.ServiceProxy('/arm_mover/safe_move', 
                                 GoToPosition)
    rospy.spin()

Updating the launch file

Just as you did with the arm_mover node, to get look_away to launch with the rest of the nodes, you will need to modify robot_spawn.launch, which can be found in ~/catkin_ws/src/simple_arm/launch. You can add the following code there:

  <!-- The look away node -->
  <node name="look_away" type="look_away" pkg="simple_arm"/>

While editing this file, it will be helpful to set max_joint_2_angle: 1.57 in arm_mover so that it isn't necessary to set it again from the command line:

  <!-- The arm mover node -->
  <node name="arm_mover" type="arm_mover" pkg="simple_arm">
    <rosparam>
      min_joint_1_angle: 0
      max_joint_1_angle: 1.57
      min_joint_2_angle: 0
      max_joint_2_angle: 1.57
    </rosparam>
  </node>